#include <math.h>

#include "fdecl.h"

void compute_positions(hybroid *h)
{
  int i;
  node *nde;
  vector temp, *r, *d, *p, *t = &h->translation;
  double declare_pntr_to_mat(rot, 3);
  rot = h->rotation;

  //Mingchen print hybroid positions
 // for (i = 0; i < h->numnodes; i++) {
	//  nde = &h->nodes[i];
	//p = &nde->posit;
	//printf("%d postion %f %f %f\n", i, p->x, p->y, p->z);
	////printf("h->sc = %f\n", h->sc);
 // }

  //Mingchen p = rot*(r+d)+t
  //Mingchen posit = rotation*(refer+displ)+translation
  
  for (i = 0; i < h->numnodes; i++) {
    nde = &h->nodes[i];
    r = &nde->refer; d = &nde->displ; p = &nde->posit;
    vplus(*r, *d, temp);
    matvecmult(rot, temp, *p);
    vinc(*t, *p);

  }

  //Mingchen print hybroid positions
 // 	printf("after calculation");
 // for (i = 0; i < h->numnodes; i++) {
	//  nde = &h->nodes[i];
	//p = &nde->posit;
	//printf("%d postion %f %f %f\n", i, p->x, p->y, p->z);
	////printf("h->sc = %f\n", h->sc);
 // }


}

void compute_translation(hybroid *h)
{
  vector temp;

  //Mingchen translation = translation + dt * net_applied_force
  
  vscale(h->dt, h->net_applied_force, temp);
  vinc(temp, h->translation);
}


void compute_rotation(hybroid *h)
{
  quaternion temp;
/* initially it was 0.001 , the other value is 0.0004*/
  unit_quaternion(h->dt * 0.004 * vlength(h->net_applied_torque),
                  h->net_applied_torque,
                  temp);

  qmult(temp, h->orientation, h->orientation);
/* DO WE REALLY NEED THIS? *****/
/* sometimes when for example forces along the x axis only occur */
/* it has the undesirable effect of cancelling the rotation */

  qnormalize(h->orientation); 

  unit_quaternion_to_rotation_matrix(h->orientation, h->rotation);
}


void compute_rotation_derivatives_and_rotation_matrix(hybroid *h)
{
  int i;
  node *nde;
  vector *f, r, temp;
  vector *nf = &h->net_applied_force;
  quaternion qtemp, *nd = &h->dorientation;
  double declare_pntr_to_mat(rot, 3);
  double pos[3][3];
  double G[3][4];
  double mtemp[3][3];

  //Mingchen G
  G[0][0] = -2.0 * h->orientation.v.x;  G[0][1] = 2.0 * h->orientation.w;
  G[0][2] = 2.0 * h->orientation.v.z;   G[0][3] = -2.0 * h->orientation.v.y;
  G[1][0] = -2.0 * h->orientation.v.y;  G[1][1] = -2.0 * h->orientation.v.z;
  G[1][2] = 2.0 * h->orientation.w;     G[1][3] = 2.0 * h->orientation.v.x;
  G[2][0] = -2.0 * h->orientation.v.z;  G[2][1] = 2.0 * h->orientation.v.y;
  G[2][2] = -2.0 * h->orientation.v.x;  G[2][3] = 2.0 * h->orientation.w;

  pos[0][0]= 0.0; pos[1][1] = 0.0; pos[2][2] = 0.0;

  rot = h->rotation;
  qzeroize(*nd);

  for (i = 0; i < h->numnodes; i++) {
      nde = &h->nodes[i];

      f = &nde->force;
 
      vset(nde->refer, r);
/* to be used in non-rigid bodies
      vplus(nde->refer, nde->displ, r);
*/

	  //Mingchen p_tilda
      pos[0][1] = -r.z; pos[0][2] = r.y;
      pos[1][0] = r.z; pos[1][2] = -r.x;
      pos[2][0] = -r.y; pos[2][1] = r.x;

/* do not forget the possible simplification */

      matmatmult(rot, pos, mtemp);
      vecmatmult(*f, mtemp, temp);
/*
      vscale(-1.0, temp, temp);
*/
      vecmat34mult(temp, G, qtemp);
      qinc(qtemp, *nd);
  }

  qscale(-1.0, *nd, *nd);
 /* the value 0.00007 was used for the insect */
  qscale(h->dt * 0.00009 , *nd, *nd);
  qinc(*nd, h->orientation);

  unit_quaternion_to_rotation_matrix(h->orientation, h->rotation);
}  

void compute_rigid_motion(hybroid *h)
{
  compute_net_applied_force_and_torque(h);

  compute_translation(h);

  compute_rotation(h);


  compute_rotation_derivatives_and_rotation_matrix(h);

}


void compute_motion(hybroid *h)
{

  compute_positions(h);

  compute_range_data_forces_exhaustive(h);


  limit_external_forces(h, 0.0001);


  unrotate_force(h);

  compute_rigid_motion(h);

  compute_superquadric_deformation_taper_bend(h);


  if(fabs(h->sc - h->sc_pr) < 0.0001){

    printf("h->sc = %f\n", h->sc);

    compute_element_deformation(h);
  }
  h->sc_pr = h->sc;

}








